#include "imu.h"

/********************静态变量定义********************/
static imu_vector4f imu_quater={0.00001f};//imu四元数
static int8_t imu_status=IMU_NOT_READY;//imu状态
static bool is_imu_get_initial=false;
static uint8_t imu_get_initial_times=20;
static imu_euler euler_initial={0};
/********************全局函数定义********************/
/**
 * @brief imu初始化
 * @return imu/i2c状态码,其中i2c的状态码见user_i2c.h,imu的状态码见各陀螺仪的头文件
 */
int8_t imu_init(void)
{
  #ifdef IMU_USE_ICM206xx
    imu_status= icm206xx_init();
  #endif
  
  #ifdef IMU_USE_BNO08X
    imu_status= bno08x_init();
  #endif

  return imu_status;
}

/**
 * @brief 获取imu角度数据
 * @param euler 欧拉角指针
 * @param is_relative 要获取的角度是否相对于系统初始化获得的角度
 * @return imu/i2c状态码,其中i2c的状态码见user_i2c.h
 */
int8_t imu_get_data(imu_euler* euler,bool is_relative)
{
  imu_euler euler_alpha;
  quaternion_to_euler(&imu_quater,&euler_alpha);
  if(is_imu_get_initial==false&&(imu_status!=IMU_NOT_READY&&imu_status!=IMU_INIT_ERROR&&imu_status>=IMU_OK))
  {
    if(imu_get_initial_times==0)
    {
      is_imu_get_initial=true;
			euler_initial.yaw=euler_alpha.yaw;
			euler_initial.pitch=euler_alpha.pitch;
			euler_initial.roll=euler_alpha.roll;
    }
    else
    {
      imu_get_initial_times--;
    }
  }
  else if(is_relative==true&&imu_get_initial_times!=0)
  {
    euler->yaw=0.0f;
    euler->pitch=0.0f;
    euler->roll=0.0f; 
  }
  else if(is_relative==true&&imu_get_initial_times==0)
  {
    euler->yaw=-(euler_alpha.yaw>=0?euler_alpha.yaw-euler_initial.yaw:euler_alpha.yaw+euler_initial.yaw);
    euler->pitch=-(euler_alpha.pitch>=0?euler_alpha.pitch-euler_initial.pitch:euler_alpha.pitch+euler_initial.pitch);
    euler->roll=-(euler_alpha.roll>=0?euler_alpha.roll-euler_initial.roll:euler_alpha.roll+euler_initial.roll);
  }
  else if(is_relative==false)
  {
    euler->yaw=euler_alpha.yaw;
    euler->pitch=euler_alpha.pitch;
    euler->roll=euler_alpha.roll;
  }
  return imu_status;
}

/**
 * @brief imu更新角度参数
 * @return imu/i2c状态码,其中i2c的状态码见user_i2c.h,imu的状态码见各陀螺仪的头文件
 */
int8_t imu_update(void)
{
  #ifdef IMU_USE_ICM206xx
    imu_vector3f gyro, accel;
    float temperature;

    imu_status= icm206xx_read_all(&gyro.x,&gyro.y,&gyro.z,&accel.x,&accel.y,&accel.z,&temperature);
    if(imu_status!=IMU_NOT_READY&&imu_status!=IMU_INIT_ERROR&&imu_status>=IMU_OK)
    {
      /*数据滤波处理*/
      #ifdef IMU_USE_FILTER

      #endif

      /*整合数据为四元数*/
      imu_math_raw_to_quaternion(&gyro,&accel,50.0f,&imu_quater);
    }
  #endif

  #ifdef IMU_USE_BNO08X
  
    imu_status= bno08x_read_all(&imu_quater.q0,&imu_quater.q1,&imu_quater.q2,&imu_quater.q3);
  #endif
  return imu_status;
}

/**
 * @brief IMU状态检查
 */
int8_t imu_status_cheak(void)
{
  /*检查imu是否初始化*/
  if(imu_status==IMU_NOT_READY||imu_status==IMU_INIT_ERROR||imu_status<IMU_OK)
  {
    #ifdef IMU_USE_BNO08X
      imu_status=bno08x_init();
    #endif 

    #ifdef IMU_USE_ICM206xx
      imu_status= icm206xx_init();
    #endif

    if(imu_status==IMU_INIT_ERROR||imu_status<IMU_OK)
    {
      imu_status=IMU_NOT_READY;
    }
  }

  return imu_status;
}
// static imu_vector3f gyro,accel;
// static float temperature;
static imu_euler euler;
int8_t imu_read_test(void)
{
  return imu_get_data(&euler,false);
}